

#include "matrix.h"

#include <stdlib.h>
#include <string.h>
#include <stdio.h>


extern int _vm_2by2_det(vm_matrix *, double *);
extern int _vm_3by3_det(vm_matrix *, double *);
extern int _vm_nbyn_det(vm_matrix *, double *);

void _vm_make_matrix_rc(char, vm_matrix *, int, vm_matrix_rc *);

int _vm_read_matrix_row(FILE *file, int elems, double *read){

  char *tmp = (char *) malloc(2048);

  char *err = fgets(tmp, 2048, file);
  if (err == NULL){
    free((void *) tmp);
    fprintf(stderr, "Error reading data from file.\n");
    return 0;
  }

  int i = 0;
  char *next_elem;
  char *cur_elem = tmp;

  for ( ; i < elems; i++){
    next_elem = strpbrk(cur_elem, ":\n");

    if (next_elem == NULL){
      free((void *) tmp);
      fprintf(stderr, "Error: not enough columns found.\n");
      fprintf(stderr, "  next_elem=%s\n", next_elem);
      fprintf(stderr, "  cur_elem=%s\n", cur_elem);
      return 0;
   }
    *next_elem = 0; // Delimit the current element.
    next_elem++;
    /* At this point cur_elem is a string which represents our matrix elem */
    read[i] = strtod(cur_elem, NULL);
    cur_elem = next_elem;
  }

  return 1;

}

/*****************************************************
 * vm_read_matrix():                                 *
 *   Read a matrix from a file; one matrix per file. *
 *                                                   *
 *****************************************************/

vm_matrix *vm_read_matrix(char *file){

  //fprintf(stderr, "[vm_read_matrix] Reading from this file: %s\n", file);
  FILE *mtx = fopen(file, "r");
  if (mtx == NULL) {
    //fprintf(stderr, "Returning 0.\n");
    return 0;
  }
  //fprintf(stderr, "[vm_read_matrix] Reading...\n");

  char *form = "matrix: %d:%d\n";

  int result;
  int rows, cols;

  result = fscanf(mtx, form, &rows, &cols);
  if (result != 2) {
    fprintf(stderr, "[vm_read_matrix] Poorly formed header.\n");
    return NULL;
  }
  vm_matrix *m = vm_make_matrix(rows, cols);

  //fprintf(stderr, "Parsing %dx%d matrix...\n", rows, cols);

  int i = 0;
  for ( ; i < rows; i++){
    if (!_vm_read_matrix_row(mtx, cols, m->data[i])){
      fprintf(stderr, "Error loading matrix from file: %s\n", file);
      return NULL;
    }
  }

  return m;

}

void vm_get_matrix_row(vm_matrix *m, int sub, vm_matrix_row *dest){

  _vm_make_matrix_rc(VM_ROW, m, sub, (vm_matrix_rc *)dest);

}

void vm_get_matrix_col(vm_matrix *m, int sub, vm_matrix_row *dest){

  _vm_make_matrix_rc(VM_COL, m, sub, (vm_matrix_rc *)dest);

}

void _vm_make_matrix_rc(char rc, vm_matrix *m, int sub, vm_matrix_rc *dest){

  if (rc == VM_ROW){
    if (sub >= m->rows){
      dest->elems = -1;
      dest->data = NULL;
    } else {
      dest->data = (double *) malloc(sizeof(double) * m->columns);
      dest->elems = m->columns;
      memcpy(dest->data, m->data[sub], sizeof(double) * m->columns);
    }
  } else if (rc == VM_COL){
    if (sub >= m->columns){
      dest->elems = -1;
      dest->data = NULL;
    } else {
      dest->data = (double *) malloc(sizeof(double) * m->rows);
      dest->elems = m->rows;
      /* Have to do this the long way. */
      int i = 0;
      for ( ; i < m->rows; i++){
	dest->data[i] = m->data[i][sub];
      }
    }
  } else {
    dest->elems = -1;
    dest->data = NULL;
  }

}

/**
 * This swaps the two given rows assuming they are actually in the matrix.
 */

int vm_swap_rows(vm_matrix *mtx, int r1, int r2){

  if (r1 < 0 || r1 >= mtx->rows) return 0;
  if (r2 < 0 || r2 >= mtx->rows) return 0;
  if (r1 == r2) return 1;

  //fprintf(stderr, "[vm_swap_rows]: [R1:R2]: [%d:%d]\n", r1, r2);

  double *tmp_row = mtx->data[r1];
  mtx->data[r1] = mtx->data[r2];
  mtx->data[r2] = tmp_row;

  return 1;

}

char vm_matrix_dot(vm_matrix_rc *v1, vm_matrix_rc *v2, double *dest){

  if (v1->elems != v2->elems) {
    *dest = 0;
    return 0;
  }

  *dest = 0;
  int i = 0;
  for ( ; i < v1->elems; i++){
    *dest += (v1->data[i] * v2->data[i]);
  }

  return 1;

}

vm_matrix *vm_matrix_mult(vm_matrix *m1, vm_matrix *m2){

  if (m1 == NULL || m2 == NULL) return NULL;
  if (m1->columns != m2->rows) return NULL;

  vm_matrix *dest = vm_make_matrix(m1->rows, m2->columns);
  if (dest == NULL) return NULL;

  int i = 0, j;
  for ( ; i < m1->rows; i++){
    vm_matrix_row cur_row;
    vm_get_matrix_row(m1, i, &cur_row);
    for ( j = 0; j < m2->columns; j++){
      vm_matrix_col cur_col;
      vm_get_matrix_col(m2, j, &cur_col);
      double temp;
      if (!vm_matrix_dot(&cur_row, &cur_col, &temp)){
	printf("Error detected multiplying matrices...\n");
      }
      dest->data[i][j] = temp;
    }
  }

  return dest;

}

vm_matrix *vm_matrix_add(vm_matrix *m1, vm_matrix *m2){

  if (m1 == NULL || m2 == NULL) return NULL;
  if (m1->columns != m2->columns) return NULL;
  if (m1->rows != m2->rows) return NULL;

  vm_matrix *res = vm_make_matrix(m1->rows, m1->columns);
  if (res == NULL) return NULL;

  int i = 0, j;
  
  for ( ; i < m1->rows; i++){
    for ( j = 0; j < m1->columns; j++){
      res->data[i][j] = m1->data[i][j] + m2->data[i][j];
    }
  }
  
  return res;

}

vm_matrix *vm_matrix_sub(vm_matrix *m1, vm_matrix *m2){

  if (m1 == NULL || m2 == NULL) return NULL;
  if (m1->columns != m2->columns) return NULL;
  if (m1->rows != m2->rows) return NULL;

  vm_matrix *res = vm_make_matrix(m1->rows, m1->columns);
  if (res == NULL) return NULL;

  int i = 0, j;
  
  for ( ; i < m1->rows; i++){
    for ( j = 0; j < m1->columns; j++){
      res->data[i][j] = m1->data[i][j] - m2->data[i][j];
    }
  }
  
  return res;

}

vm_matrix *vm_transpose(vm_matrix *mtx){

  if (mtx == NULL) return NULL;
  vm_matrix *mtx_trans = vm_make_matrix(mtx->columns, mtx->rows);
  if (mtx_trans == NULL) return NULL;

  int i = 0;
  int j = 0;
  for ( ; i < mtx_trans->rows; i++){
    j = 0;
    for ( ; j < mtx_trans->columns; j++){
      mtx_trans->data[i][j] = mtx->data[j][i];
    }
  }

  return mtx_trans;

}

int vm_determinant(vm_matrix *mtx, double *dest){

  if (mtx == NULL || dest == NULL) return 0;
  if (mtx->rows != mtx->columns) return 0;
  
  if (mtx->rows == 2) return _vm_2by2_det(mtx, dest);
  if (mtx->rows == 3) return _vm_3by3_det(mtx, dest);

  return _vm_nbyn_det(mtx, dest);

}

vm_matrix *vm_inverse_old(vm_matrix *mtx){

  if (mtx == NULL) return 0;
  if (mtx->rows != mtx->columns) return NULL;

  double det;
  vm_determinant(mtx, &det);

  vm_matrix *adj = vm_adjoint(mtx);
  vm_matrix *inv = vm_make_matrix(mtx->rows, mtx->columns);
  int r, c;
  for ( r = 0; r < inv->rows; r++)
    for ( c = 0; c < inv->columns; c++)
      inv->data[r][c] = adj->data[r][c] / det;
  
  vm_free_matrix(adj);
  return inv;

}

vm_matrix *vm_inverse(vm_matrix *mtx){

  if (mtx == NULL) return NULL;
  if (mtx->rows != mtx->columns) return NULL;

  /* First augment the given matrix with the corresponding identity matrix. */
  /* N x 2N matrix.                                                         */
  vm_matrix *aug = vm_make_matrix(mtx->rows, 2 * mtx->columns);
  
  /* Copy the first matrix in and then copy the identity matrix in.         */

  int i = 0, j;
  for ( ; i < mtx->rows; i++){
    for ( j = 0; j < mtx->columns; j++){
      aug->data[i][j] = mtx->data[i][j];
    }
  }

  int k = 0;
  for ( ; k < mtx->rows; k++){
    aug->data[k][i + k] = 1;
  }

  /* Reduced row echelon form.                                              */
  vm_matrix *aug_rref = vm_rref(aug);

  /* Now copy the right hand side out for our inverse.                      */
  vm_matrix *inverse = vm_make_matrix(mtx->rows, mtx->columns);
  
  for ( i = 0; i < inverse->rows; i++){
    for ( j = 0; j < inverse->columns; j++){
      inverse->data[i][j] = aug_rref->data[i][mtx->columns + j];
    }
  }

  return inverse;

}

vm_matrix *vm_vector_to_matrix(vm_vector *v){

  if (v == NULL) return NULL;

  vm_matrix *mtx = vm_make_matrix(1, 4);
  mtx->data[0][0] = v->x;
  mtx->data[0][1] = v->y;
  mtx->data[0][2] = v->z;
  mtx->data[0][3] = v->w;
  return mtx;

}

void vm_matrix_to_vector(vm_matrix *mtx, vm_vector *dest){

  dest->x = mtx->data[0][0];
  dest->y = mtx->data[0][1];
  dest->z = mtx->data[0][2];
  dest->w = mtx->data[0][3];

}

vm_matrix *vm_make_matrix(int rows, int columns){

  if (rows == 0 || columns == 0) return NULL;

  vm_matrix *new = (vm_matrix *) malloc(sizeof(vm_matrix));
  new->rows = rows;
  new->columns = columns;

  new->data = (double **) malloc(sizeof(double *) * rows);
  int i = 0;
  for ( ; i < rows; i++){
    new->data[i] = (double *) malloc(sizeof(double) * columns);
  }
  
  /* Zero the matrix. */
  int r, c;
  for (r = 0; r < rows; r++)
    for ( c = 0; c < columns; c++)
      new->data[r][c] = 0.0;

  return new;

}

void vm_free_matrix(vm_matrix *mtx){

  if (mtx == NULL) return;
  
  int i;
  for ( i = 0; i < mtx->rows; i++){
    free((void *) mtx->data[i]);
  }

  free((void *) mtx->data);

}

void vm_free_only_matrix(vm_matrix *mtx){

  if (mtx == NULL) return;

  free((void *) mtx);

}

void vm_fprint_matrix(FILE *stream, vm_matrix *mtx, char *prepend){

  if (stream == NULL || mtx == NULL || prepend == NULL) return;

  int len = 15 * mtx->columns;
  char *blank = (char *) malloc(len);
  memset(blank, ' ', len - 1);

  int r = 0;
  int c = 0;
  fprintf(stream, "%s+-%s-+\n", prepend, blank);
  
  for ( ; r < mtx->rows; r++){
    fprintf(stream, "%s|", prepend);
    for ( c = 0; c < mtx->columns; c++){
      fprintf(stream, " %-14f", mtx->data[r][c]);
    }
    fprintf(stream, " |\n");
  }
  fprintf(stream, "%s+-%s-+\n", prepend, blank);
  free(blank);

}

void vm_fprint_cr_matrix(FILE *f, vm_matrix *mtx){

  if (f == NULL || mtx == NULL) return;
  
  fprintf(f, "matrix: %d:%d\n", mtx->rows, mtx->columns);

  int i = 0, j;
  for ( ; i < mtx->rows; i++){
    j = 1;
    fprintf(f, "%f", mtx->data[i][0]);
    for ( ; j < mtx->columns; j++){
      fprintf(f, ":%f", mtx->data[i][j]);
    }
    fprintf(f, "\n");
  }

}

void vm_print_matrix(vm_matrix *mtx, char *prepend){

  vm_fprint_matrix(stdout, mtx, prepend);

}

void vm_fprint_octave_matrix(FILE *stream, vm_matrix *mtx){

  if (mtx == NULL) return;

  fprintf(stream, "[");

  int i = 0, j;
  for ( ; i < mtx->rows; i++){
    fprintf(stream, "[");
    for ( j = 0; j < mtx->columns; j++){
      fprintf(stream, "%f,", mtx->data[i][j]);
    }
    fprintf(stream, "];");
  }
  fprintf(stream, "]\n");

}

void vm_print_cr_matrix(vm_matrix *mtx){

  vm_fprint_cr_matrix(stdout, mtx);

}

void vm_print_matrix_rc(vm_matrix_rc *mtx){
  
  if (mtx == NULL){
    printf("[ NULL ]\n");
    return;
  }

  printf("[");
  int i = 0;
  for ( ; i < mtx->elems; i++){
    printf(" %-14f", mtx->data[i]);
  }
  printf(" ]\n");
  
}
